#include<SoftwareSerial.h>
#include <Servo.h> 
int MIN1=6;//定义电机引脚
int MIN2=9;
int MIN3=10;
int MIN4=11;

int RIN1=A0;//定义红外循迹引脚
int RIN2=A1;
int RIN3=A2;
int RIN4=A3;

int MG=5;//定义舵机引脚

Servo myservo; 

SoftwareSerial BT(2,3);
int X=0;
int A;

void setup()
{
  Serial.begin(9600);
  Serial.println("蓝牙连接正常");//调试蓝牙
  BT.begin(9600);//设置蓝牙
  pinMode(MIN1,OUTPUT);
  pinMode(MIN2,OUTPUT);
  pinMode(MIN3,OUTPUT);
  pinMode(MIN4,OUTPUT);
  pinMode(RIN1,OUTPUT);
  pinMode(RIN2,OUTPUT);
  pinMode(RIN3,OUTPUT);
  pinMode(RIN4,OUTPUT);
  myservo.attach(MG);
}


void go()//快速前进
{
  myservo.write(90);
  digitalWrite(MIN1,HIGH);
  digitalWrite(MIN2,LOW);
  digitalWrite(MIN3,HIGH);
  digitalWrite(MIN4,LOW);
}

void slow(int pwm)//慢速前进，速度可调
{
  myservo.write(90);
  digitalWrite(MIN1,HIGH);
  digitalWrite(MIN2,LOW);
  analogWrite(MIN1,pwm);
  analogWrite(MIN2,0);
  digitalWrite(MIN3,HIGH);
  digitalWrite(MIN4,LOW);
  analogWrite(MIN3,pwm);
  analogWrite(MIN4,0);
}

void pause()//停止
{
  digitalWrite(MIN1,LOW);
  digitalWrite(MIN2,LOW);
  digitalWrite(MIN3,LOW);
  digitalWrite(MIN4,LOW);
}

void back()//后退
{
  myservo.write(90);
  digitalWrite(MIN1,LOW);
  digitalWrite(MIN2,HIGH);
  digitalWrite(MIN3,LOW);
  digitalWrite(MIN4,HIGH);
}

void bigright()//大右转程序
{
  slow(80);
  myservo.write(20);
  delay(100);
}

void right()//右转程序
{
  slow(160);
  myservo.write(70);
    delay(100);
}

void bigleft()//大左转程序
{
  slow(80);
  myservo.write(160);
    delay(100);
}

void left()//左转程序
{
  slow(160);
  myservo.write(110);
    delay(100);
}


void track()//循迹模式
{
  int date0;
  int date1;
  int date2;
  int date3;
  int a=0;
  int i;

  date0=digitalRead(RIN1);
  date1=digitalRead(RIN2);
  date2=digitalRead(RIN3);
  date3=digitalRead(RIN4);
     myservo.write(90);
     
  if(date0==1&&date1==1&&date2==1&&date3==1)
  a=1;
  if(date0==1&&date1==1&&date2==1&&date3==0)
  a=2;
  if(date0==1&&date1==1&&date2==0&&date3==1)
  a=3;
  if(date0==1&&date1==1&&date2==0&&date3==0)
  a=4;
  if(date0==1&&date1==0&&date2==1&&date3==1)
  a=5;
  if(date0==1&&date1==0&&date2==1&&date3==0)
  a=6;
  if(date0==1&&date1==0&&date2==0&&date3==1)
  a=7;
  if(date0==1&&date1==0&&date2==0&&date3==0)
  a=8;
  if(date0==0&&date1==1&&date2==1&&date3==1)
  a=9;
  if(date0==0&&date1==1&&date2==1&&date3==0)
  a=10;
  if(date0==0&&date1==1&&date2==0&&date3==1)
  a=11;
  if(date0==0&&date1==1&&date2==0&&date3==0)
  a=12;
  if(date0==0&&date1==0&&date2==1&&date3==1)
  a=13;
  if(date0==0&&date1==0&&date2==1&&date3==0)
  a=14;
  if(date0==0&&date1==0&&date2==0&&date3==1)
  a=15;
  if(date0==0&&date1==0&&date2==0&&date3==0)
  a=16;
  switch(a)
  {
  case 4:bigleft();break;
  case 8:bigleft();break;
  case 12:left();break;
  case 13:bigright();break;
  case 14:right();break;
  case 15:bigright();break;
  case 16:go();break;
  default:pause();
  }
}

void bluetooth()
{
   myservo.write(90);
  if(Serial.available())
  {
    X = Serial.read();
    BT.print(X);
  }

  if(BT.available())
  {
    X = BT.read();
    Serial.print(X);
  }
  if(X==53)    //后退
  back();
  if(X==54)   //停止
  pause();
  if(X==55)   //前进
  go();
  if(X==56)   //右转
  right();
  if(X==57)   //左转
  left();
  if(X==58)  //进入循迹模式
  track();
  
}
void loop()
{
  bluetooth();
}
